/*
 * Swarm.cpp
 *
 *  Created on: 6 mei 2013
 *      Author: Wietse
 */

#include "Swarm.h"

#include <algorithm>
#include <iostream>
#include <stdio.h>
#include <sstream>


/**
 * Default constructor of the swarm
 */
Swarm::Swarm()
{
	srand(time(0));

	spawnQuadcopters();

	spawnCrawlers();

	// Create the basestation
	basestation = BaseStation(1, Position(25.0, 25.0));

	// Clear the output file for the coverage
	clearFile();
	coverage_timer = 0.0;
}

/**
 * Default destructor
 */
Swarm::~Swarm()
{
}

std::vector<Message>* Swarm::getMessages()
{
	return &messages;
}

/**
 * Add a message to the messages vector
 */
void Swarm::addMessage(Message message)
{
	messages.push_back(message);
}

/**
 * Remove a message of the vector Messages when it is present
 */
void Swarm::removeMessage(Message *message)
{
	// Iterate over the messages vector
	for(std::vector<int>::size_type i = 0; i < messages.size(); i++)
	{
		// Check if this message has to be removed
		if(message == &(messages.at(i)))
		{
			// Remove the message: destructor is called automatically
			messages.erase(messages.begin() + i);
		}
	}
}

/**
 * Update the swarm
 */
void Swarm::update(double deltaTime)
{
	// Update messages
	for(unsigned int i = 0; i < messages.size(); i++)
	{
		messages.at(i).update(this);
	}

	// Update Quadcopters
	for(int i = 0; i < NUM_OF_QUADCOPTERS; i++)
	{
		quadcopters[i].update(this, deltaTime, quadcopters);
	}

	// Update Crawlers
	for(int i = 0; i < NUM_OF_CRAWLERS; i++)
	{
		crawlers[i].update(this, deltaTime);
		crawlers[i].setCovered(false);

		// Iterate over the quadcopters array
		for(int j = 0; j < NUM_OF_QUADCOPTERS; j++)
		{
			// Check if the Crawlers is in range of the quadcopter
			if(crawlers[i].getPosition()->distance(quadcopters[j].getPosition()) < AGENT_CRAWLER_DETECTION_RANGE)
			{
				crawlers[i].setCovered(true);
				//break;
			}
		}
	}

	// Update the basestation
	basestation.update(this, deltaTime);

	// Output the coverage of the Crawlers if the timer has expired
	coverage_timer += deltaTime;
	if(coverage_timer > COVERAGE_TIME)
	{
		outputCoverage();
		coverage_timer = 0.0;
	}
}

/**
 * Display the swarm on screen
 */
void Swarm::display()
{
	// Display Crawlers
	for(int i = 0; i < NUM_OF_CRAWLERS; i++)
	{
		crawlers[i].display();
	}

	// Display Quadcopters
	glColor3f(0.0, 0.0, 0.0);	// Black
	for(int i = 0; i < NUM_OF_QUADCOPTERS; i++)
	{
		quadcopters[i].display();
	}

	// Display the basestation
	glColor3f(0.0, 0.0, 1.0);	// Blue
	basestation.display();
}

/**
 * Measure if there are Crawlers nearby
 */
std::vector<Crawler *> Swarm::senseCrawlers(Position *pos)
{
	// Declare the result vector
	std::vector<Crawler *> result;

	// Iterate over all the messages
	for(int i = 0; i < NUM_OF_CRAWLERS; i++)
	{
		// Check if the message has reached the object
		bool inRange = crawlers[i].inRange(pos);

		if(inRange)
		{
			// Message has reached the object. Add it to the result vector
			result.push_back(&crawlers[i]);
		}
	}
	return result;
}

/**
 * Clear the output file Coverage.txt
 */
void Swarm::clearFile()
{
	// Get filename and identifier
	coverage_filename = "Coverage.txt";

	// Open File
	std::ofstream output;
	output.open(coverage_filename.c_str(), std::ios::out);

	// Print ID and time
	output << "Swarm simulator: Coverage of crawlers";
	output << std::endl;

	// Close file
	output.close();
}

/**
 * Print the current coverage to the output file
 */
void Swarm::outputCoverage()
{
	int num_covered = 0;

	// Count the number of crawlers that is covered
	for(int i = 0; i < NUM_OF_CRAWLERS; i++)
	{
		if(crawlers[i].getCovered() == true)
		{
			num_covered++;
		}
	}

	// Open File
	std::ofstream output;
	output.open(coverage_filename.c_str(), std::ios::app);

	// Output the number of covered crawlers with a timestamp
	outputTime(output);
	output << num_covered << std::endl;

	// Close the file
	output.close();
}

/**
 * Write the timestamp to the output file
 */
void Swarm::outputTime(std::ofstream& output)
{
	// Retrieve current time and print it
	time_t t = time(0);
	struct tm *now = localtime(&t);

	output 	<< now->tm_hour << ":"
			<< now->tm_min << ":"
			<< now->tm_sec << " ;";
}

/**
 * Spawn the quadcopters at random positions
 */
void Swarm::spawnQuadcopters()
{
	// Spawn every quadcopter
	for(unsigned int i = 0; i < NUM_OF_QUADCOPTERS; i++)
	{

		// Get random doubles for x and z
	/*
		double f = (double)rand() / RAND_MAX;
		double x = 0.0 + f * (100.0 - 0.0);

		f = (double)rand() / RAND_MAX;
		double z = 0.0 + f * (100.0 - 0.0);
*/
		// Create the quadcopter
		quadcopters[i] = Quadcopter(i+1, this, Position(50.0,50.0));
	}
}

/**
 * Spawn the quadcopters at random positions
 */
void Swarm::spawnCrawlers()
{
	// Create the crawlers
	for(int i = 0; i < NUM_OF_CRAWLERS; i++)
	{
		//Get random doubles for x and z
		double f = (double)rand() / RAND_MAX;
		double x = 0.0 + f * (100.0 - 0.0);

		f = (double)rand() / RAND_MAX;
		double z = 0.0 + f * (100.0 - 0.0);

		// Create the Crawlers
		crawlers[i] = Crawler(i+1, Position(x, z));
	}
}
